
#include <algorithm>
#include "deformationMesh.h"
#include "weightsScheme.h"

//namespace MLS {

DeformationMesh::DeformationMesh() {
   Deformation::Deformation();
   
   model = 0;
   R = MatrixCPPL(3,3);
   A = MatrixCPPL(4,4);
   mW = MatrixCPPL(4,4);
   mQ = MatrixCPPL(4,4);
   C1 = MatrixCPPL(4,4);
   C3 = MatrixCPPL(4,4);
   mK = MatrixCPPL(3,3);
   mI = MatrixCPPL(3,3); mI.identity();
   T = VectorCPPL(3);
   //~ rbf = 0;
   
   rotation_mask = AXISANGLE;
   translation_mask = WCENTROID;
}

DeformationMesh::DeformationMesh(Mesh *m) {
   
   assert(m != 0);
   model = m;
   R = MatrixCPPL(3,3);
   A = MatrixCPPL(4,4);
   mW = MatrixCPPL(4,4);
   mQ = MatrixCPPL(4,4);
   C1 = MatrixCPPL(4,4);
   C3 = MatrixCPPL(4,4);
   mK = MatrixCPPL(3,3);
   mI = MatrixCPPL(3,3); mI.identity();
   T = VectorCPPL(3);
   //~ rbf = 0;
   
   rotation_mask = AXISANGLE;
   translation_mask = WCENTROID;
}

DeformationMesh::~DeformationMesh() {
   
}

void DeformationMesh::setMesh(Mesh *m) {
   
   assert(m != 0);
   model = m;
}

void DeformationMesh::setting(const Constraints *constraints) {
   
   assert(model != 0);
   
   ipositions.resize(model->nvertices());
   inormals.resize(model->nvertices());
   Vertex_iterator vi = model->vertices_begin();
   for (unsigned int i=0; i<ipositions.size(); ++i, ++vi) {
      ipositions[i] = vi->point();
      inormals[i] = vi->normal();
   }
   
   Deformation::setting(&ipositions, model->nvertices(), &constraints->positions);
   
   // Compute the weights table
   if (WeightsTable != 0) delete []WeightsTable;
   
   unsigned int nminusone = std::count(constraints->ids.begin(), constraints->ids.end(), -1);
   if (nminusone == 0) // If all constraints are associated with joints
      WeightsTable = ::compute_WeightsTable(model, constraints);
   else if (nminusone < constraints->ids.size()) // If constraints are represented a trihedron
      WeightsTable = ::compute_WeightsTable_Trihedrons(model, constraints);
   else
      WeightsTable = 0;
   
   Deformation::initialize();
   
   //~ cout<<"opaaaaaaaaaaa"<<endl;
   //~ if (rbf != 0) delete rbf;
   //~ rbf = new RBFfunction;
   //~ rbf->setting(constraints);
   //~ cout<<"opaaaaaaaaaaa rbf"<<endl;
}

void DeformationMesh::execute_deformation(const Constraints *constraints) {
   
   d_constraintsPos = &constraints->positions; // positions of deformed constraints 
   if (rotation_mask == BLENDINGROT) {
      for (unsigned int i=0; i<model->nvertices(); ++i) {
         compRotTra_simple(i);
         //~ qstarList[i][0]=qstar[0]; qstarList[i][1]=qstar[1]; qstarList[i][2]=qstar[2];
      }
   }
   else if (rotation_mask == AXISANGLE) {
      for (unsigned int i=0; i<model->nvertices(); ++i) {
         compRotTra_axisangle(i);
      }
   }
   else if (rotation_mask == UNITQUAT) {
      for (unsigned int i=0; i<model->nvertices(); ++i)
         compRotTra_unitquat(i);
   }
   //~ else if (viewitems_mask == DUALQUAT) {
      //~ for (unsigned int i=0; i<model->nvertices(); ++i)
         //~ compRotTra_unitdualquat(i);
   //~ }
   
   
   /*
   rbf->compute_lambdas(constraints);
   d_constraintsPos = &constraints->positions; // positions of deformed constraints 
   Vertex_iterator vi = model->vertices_begin(); unsigned int i=0, iv=0;
   while (vi != model->vertices_end()) {
      qstar = rbf->evalf(pstarList[i]);
      qstarList[i][0]=qstar[0]; qstarList[i][1]=qstar[1]; qstarList[i][2]=qstar[2];
      //~ cout<<"q*: "<<qstar<<endl;
      //~ compute_qstar_points(i); 
      computeCorrelationMatrix_points(i); 
      eigenvalue = computeLargestRootOfQuarticEquation() + traceM; 
      compute_u();
      
      // Rotate the vertex position
      computeRotation(&vhatList[i][0]);
      float *p = &model->vpositions[vi->id()];
      p[0] = qstar[0]+rv[0];
      p[1] = qstar[1]+rv[1];
      p[2] = qstar[2]+rv[2];
      
      // Rotate the normal vector
      computeRotation(&model->i_normals[iv]);
      model->normals[iv  ] = rv[0];
      model->normals[iv+1] = rv[1];
      model->normals[iv+2] = rv[2];
      
      ++vi; ++i; iv+=3;
   }
   */
}

void DeformationMesh::compRotTra_simple(unsigned int i) {
   
   compute_qstar_points(i);
   
   vector<Vector3> vri(nconstraints, Vector3(0,0,0));
   const Number *pihatV = &phatTable[i*3]; /// pointer to phat of vertex i
   unsigned tsize=3*texSizetexSize;
   for (unsigned int j=0, row=0; j<nconstraints; ++j, row+=tsize) {
      //~ W = weight(i, j);
      const Number *pjhat = &pihatV[row]; // phat = pi - p*
      //~ const Number *q = &(*d_constraintsPos)[j][0];
      const Point3 &q = (*d_constraintsPos)[j];
      qjhat[0] = q[0]-qstar[0]; // qhat = qi - q*
      qjhat[1] = q[1]-qstar[1]; 
      qjhat[2] = q[2]-qstar[2]; 
      Vector3 pi(pjhat[0], pjhat[1], pjhat[2]);
      Vector3 qi(qjhat[0], qjhat[1], qjhat[2]);
      Number dot = pi*qi;
      //~ if (dot > 1.0) dot = 1.0;
      //~ if (dot <-1.0) dot =-1.0;
      const Number &norpi = sqrt(pi*pi);
      const Number &norqi = sqrt(qi*qi);
      const Number &angle = acos(dot/(norpi*norqi));
      Vector3 axis = cross_product(pi, qi);
      axis = axis/sqrt(axis*axis);
      vri[j] = axis*angle;
   }
   
   Number swi = 0.0;
   Vector3 swri(0,0,0);
   for (unsigned int j=0, row=0; j<nconstraints; ++j, row+=tsize) {
      W = weight(i, j);
      const Number *pjhat = &pihatV[row]; // phat = pi - p*
      Vector3 pi(pjhat[0], pjhat[1], pjhat[2]);
      const Number &norpi = sqrt(pi*pi);
      swi  = swi + W*norpi;
      swri = swri + vri[j]*W*norpi;
   }
   Vector3 r = swri/swi;
   
   {
   Number angle = sqrt(r*r);
   r = r/angle;
   const Number &x=r[0], &y=r[1], &z=r[2];
   Number c = cos(angle);
   Number s = sin(angle);
   Number t = 1.0f - c;
   R(0,0)=t*x*x+c;   R(0,1)=t*x*y-z*s; R(0,2)=t*x*z+y*s; 
   R(1,0)=t*x*y+z*s; R(1,1)=t*y*y+c;   R(1,2)=t*y*z-x*s; 
   R(2,0)=t*x*z-y*s; R(2,1)=t*y*z+x*s; R(2,2)=t*z*z+c; 
   //~ cout<<"Rot ok: "<<endl<<R<<endl;
   }
   
   /// Deform positions and normals
   Vertex_iterator vi = model->vertices_begin();
   const Vector3 &pi = vhatList[i];
   VectorCPPL xi(3);
   xi(0) = pi[0]; xi(1) = pi[1]; xi(2) = pi[2];
   VectorCPPL xd = R*xi;
   Point3 &p = vi[i].point();
   p = Point3(xd(0)+qstar[0], xd(1)+qstar[1], xd(2)+qstar[2]);
   
   const Vector3 &ni = inormals[i];
   xi(0) = ni[0]; xi(1) = ni[1]; xi(2) = ni[2];
   xd = R*xi;
   Vector3 &n = vi[i].normal();
   n = Vector3(xd(0),xd(1),xd(2));
}

void DeformationMesh::compRotTra_axisangle(unsigned int i) {
   
   Vertex_iterator vi = model->vertices_begin();
   
   compute_qstar_points(i);
   computeCorrelationMatrix_points(i);
   eigenvalue = computeLargestRootOfQuarticEquation() + traceM;
   compute_u();
   
   // Rotate the vertex position
   computeRotation(&vhatList[i][0]);
   Point3 &p = vi[i].point();
   p = Point3(qstar[0]+rv[0], qstar[1]+rv[1], qstar[2]+rv[2]);
   
   // Rotate the normal vector
   computeRotation(&inormals[i][0]);
   Vector3 &n = vi[i].normal();
   n = Vector3(rv[0], rv[1], rv[2]);
}

void DeformationMesh::compRotTra_unitquat(unsigned int i) {
   
   compute_qstar_points(i); 
   computeCorrelationMatrix_points(i); 
   
   // Build the 4x4 matrix
   A(0,0)=Mf0[0]+Mf1[1]+Mf2[2]; A(0,1)=Mf2[1]-Mf1[2];        A(0,2)=Mf0[2]-Mf2[0];         A(0,3) = Mf1[0]-Mf0[1];
   A(1,0)=A(0,1);               A(1,1)=Mf0[0]-Mf1[1]-Mf2[2]; A(1,2)=Mf0[1]+Mf1[0];         A(1,3) = Mf2[0]+Mf0[2];
   A(2,0)=A(0,2);               A(2,1)=A(1,2);               A(2,2)=-Mf0[0]+Mf1[1]-Mf2[2]; A(2,3) = Mf1[2]+Mf2[1];
   A(3,0)=A(0,3);               A(3,1)=A(1,3);               A(3,2)=A(2,3);                A(3,3) =-Mf0[0]-Mf1[1]+Mf2[2];
   
   // compute the eigenvalues/eigenvectors
   vector<double> er, ei;
   vector<VectorCPPL> vr, vi;
   A.dgeev(er, ei, vr, vi);
   
   // get the eigenvector corresponding to the largest eignevalue
   int lmax = 0, rmax = 2, imax;
   if (er[0] < er[1]) lmax = 1;
   if (er[2] < er[3]) rmax = 3;
   (er[lmax] < er[rmax])? (imax=rmax):(imax=lmax);
   const VectorCPPL &q = vr[imax]; /// a quaternion in format q = [a x y z]
   
   /// Build the rotation matrix
   VectorCPPL qvv(3);
   qvv(0)=q(1); qvv(1)=q(2); qvv(2)=q(3);
   const Number qv[]={q(1),q(2),q(3)};
   buildK(qv);
   R = (q(0)*q(0) - CPPL::t(qvv)*qvv)*mI + 2.0*qvv*CPPL::t(qvv) + 2.0*q(0)*mK;
   
   //~ const Number &q02 = q(0)*q(0); const Number &q12 = q(1)*q(1);
   //~ const Number &q22 = q(2)*q(2); const Number &q32 = q(3)*q(3);
   //~ R(0,0)=q02+q12-q22-q32;           R(0,1)=2.0*(q(1)*q(2)-q(0)*q(3)); R(0,2)=2.0*(q(1)*q(3)+q(0)*q(2)); 
   //~ R(1,0)=2.0*(q(2)*q(1)+q(0)*q(3)); R(1,1)=q02-q12+q22-q32;           R(1,2)=2.0*(q(2)*q(3)-q(0)*q(1)); 
   //~ R(2,0)=2.0*(q(3)*q(1)-q(0)*q(2)); R(2,1)=2.0*(q(3)*q(2)+q(0)*q(1)); R(2,2)=q02-q12-q22+q32; 
   //~ cout<<"Rot ok: "<<endl<<R<<endl;
   
   /// Deform positions and normals
   Vertex_iterator vit = model->vertices_begin();
   const Vector3 &pi = vhatList[i];
   VectorCPPL xi(3);
   xi(0) = pi[0]; xi(1) = pi[1]; xi(2) = pi[2];
   VectorCPPL xd = R*xi;
   Point3 &p = vit[i].point();
   p = Point3(xd(0)+qstar[0], xd(1)+qstar[1], xd(2)+qstar[2]);
   
   const Vector3 &ni = inormals[i];
   xi(0) = ni[0]; xi(1) = ni[1]; xi(2) = ni[2];
   xd = R*xi;
   Vector3 &n = vit[i].normal();
   n = Vector3(xd(0),xd(1),xd(2));
}

void DeformationMesh::compRotTra_unitdualquat(unsigned int i) {
   
   compute_qstar_points(i); 
   //~ cout<<"loop: "<<endl;
   C1.zero(); C3.zero();
   const unsigned int &N = nconstraints;
   for (unsigned int j=0; j<N; ++j) {
      //~ const Number *p = &(*i_constraintsPos)[j][0];
      //~ const Number *q = &(*d_constraintsPos)[j][0];
      const Point3 &p = (*i_constraintsPos)[j];
      const Point3 &q = (*d_constraintsPos)[j];
      Number pp[] = {p[0]*0.5, p[1]*0.5, p[2]*0.5, 0.0};
      Number qq[] = {q[0]*0.5, q[1]*0.5, q[2]*0.5, 0.0};
      W = weight(i, j);
      //~ cout<<"p "<<p[0]<<" "<<p[1]<<" "<<p[2]<<endl;
      //~ cout<<"q "<<q[0]<<" "<<q[1]<<" "<<q[2]<<endl;
      buildQ(qq); buildW(pp);
      //~ cout<<"W "<<W<<" (i,j) "<<i<<" "<<j<<endl;
      //~ cout<<"Q "<<mQ<<endl;
      //~ cout<<"W "<<mW<<endl;
      C1 += W*mQ*mW;
      C3 += W*(mW-mQ);
      //~ cout<<"C1 "<<C1<<endl;
      //~ cout<<"C3 "<<C3<<endl;
   }
   C1 *= -2.0;
   C3 *= 2.0;
   //~ A = (1.0/Number(4.0*N))*CPPL::t(C3)*C3 - C1;
   A = 0.5*(CPPL::t(C3)*C3 - C1 - CPPL::t(C1));
   
   cout<<"A: "<<endl<<A<<endl;
   
   // compute the eigenvalues/eigenvectors
   vector<double> er, ei;
   vector<VectorCPPL> vr, vi;
   A.dgeev(er, ei, vr, vi);
   
   //~ cout<<"ok "<<er[0]<<" "<<er[1]<<" "<<er[2]<<" "<<er[3]<<endl;
   
   // get the eigenvector corresponding to the largest eignevalue
   int lmax = 0, rmax = 2, imax;
   if (er[0] < er[1]) lmax = 1;
   if (er[2] < er[3]) rmax = 3;
   (er[lmax] < er[rmax])? (imax=rmax):(imax=lmax);
   const VectorCPPL &q = vr[imax];
   
   //~ cout<<"q (imax)"<<imax<<" : "<<q(0)<<" "<<q(1)<<" "<<q(2)<<" "<<q(3)<<endl;
   //~ // build the rotation matrix
   const Number ro[]={q(0),q(1),q(2)};
   buildK(ro);
   VectorCPPL qv(3);
   qv(0)=q(0); qv(1)=q(1); qv(2)=q(2);
   R = (q(3)*q(3) - CPPL::t(qv)*qv)*mI + 2.0*qv*CPPL::t(qv) + 2.0*q(3)*mK;
   cout<<"R dual quat: "<<endl<<R<<endl;
   
   //~ cout<<"Rot: "<<endl<<CPPL::t(R)*R<<endl;
   
   //~ const Number &q02 = q(0)*q(0); const Number &q12 = q(1)*q(1);
   //~ const Number &q22 = q(2)*q(2); const Number &q32 = q(3)*q(3);
   //~ R(0,0)=q02+q12-q22-q32;           R(0,1)=2.0*(q(1)*q(2)-q(0)*q(3)); R(0,2)=2.0*(q(1)*q(3)+q(0)*q(2)); 
   //~ R(1,0)=2.0*(q(2)*q(1)+q(0)*q(3)); R(1,1)=q02-q12+q22-q32;           R(1,2)=2.0*(q(2)*q(3)-q(0)*q(1)); 
   //~ R(2,0)=2.0*(q(3)*q(1)-q(0)*q(2)); R(2,1)=2.0*(q(3)*q(2)+q(0)*q(1)); R(2,2)=q02-q12-q22+q32; 
   
   // translation
   buildW(ro);
   T = CPPL::t(mW)*(-1.0)*C3*q;
   //cout<<"T: "<<T<<endl;
   
   
   Vertex_iterator vit = model->vertices_begin();
   const Vector3 &pi = vhatList[i];
   VectorCPPL xi(3);
   xi(0) = pi[0]; xi(1) = pi[1]; xi(2) = pi[2];
   VectorCPPL xd = R*xi + T;
   Point3 &p = vit[i].point();
   p = Point3(xd(0), xd(1), xd(2));
   
   const Vector3 &ni = inormals[i];
   xi(0) = ni[0]; xi(1) = ni[1]; xi(2) = ni[2];
   xd = R*xi;
   Vector3 &n = vit[i].normal();
   n = Vector3(xd(0),xd(1),xd(2));
}

void DeformationMesh::buildQ(const Number v[]) {
   
   buildK(v);
   mK = v[3]*mI + mK;
   mQ(0,0) = mK(0,0); mQ(0,1) = mK(0,1); mQ(0,2) = mK(0,2); mQ(0,3) = v[0];
   mQ(1,0) = mK(1,0); mQ(1,1) = mK(1,1); mQ(1,2) = mK(1,2); mQ(1,3) = v[1];
   mQ(2,0) = mK(2,0); mQ(2,1) = mK(2,1); mQ(2,2) = mK(2,2); mQ(2,3) = v[2];
   mQ(3,0) =-v[0];    mQ(3,1) =-v[1];    mQ(3,2) =-v[2];    mQ(3,3) = v[3];
}

void DeformationMesh::buildW(const Number v[]) {
   
   buildK(v);
   mK = v[3]*mI - mK;
   mW(0,0) = mK(0,0); mW(0,1) = mK(0,1); mW(0,2) = mK(0,2); mW(0,3) = v[0];
   mW(1,0) = mK(1,0); mW(1,1) = mK(1,1); mW(1,2) = mK(1,2); mW(1,3) = v[1];
   mW(2,0) = mK(2,0); mW(2,1) = mK(2,1); mW(2,2) = mK(2,2); mW(2,3) = v[2];
   mW(3,0) =-v[0];    mW(3,1) =-v[1];    mW(3,2) =-v[2];    mW(3,3) = v[3];
}

void DeformationMesh::buildK(const Number v[]) {
   
   mK(0,0) = 0.0;  mK(0,1) =-v[2]; mK(0,2) = v[1]; 
   mK(1,0) = v[2]; mK(1,1) = 0.0;  mK(1,2) =-v[0]; 
   mK(2,0) =-v[1]; mK(2,1) = v[0]; mK(2,2) = 0.0; 
}

//} // end MLS namespace
